#include "SineWarp.h"
#include "DecoLogger.h"



SineWarp::SineWarp(double amp, double phase, double period, double constant, double straight, double halfRatio, bool mod)
{
	setSineFunction(amp, phase, period, constant, straight, halfRatio, mod);
}

SineWarp::~SineWarp()
{

}

void SineWarp::setSineFunction(double amp, double phase, double period, double constant, double straight, double halfRatio, bool mod)
{
	double sinePeriod = period;

	mAmp = amp;
	mPhase = phase;
	mPeriod = period;
	mConst = constant;
	mHalfRatio = halfRatio + 0.5;
	mMod = false;
	mStraightLength = straight;
	mTotalPeriod = mPeriod + 2.0 * mStraightLength;

	mPeriod *= sinePeriod / mTotalPeriod;
	mStraightLength *= sinePeriod / mTotalPeriod;

	mTotalPeriod = mPeriod + 2.0 * mStraightLength;
}

void SineWarp::SetOriginalKnotsOnTimeAxis(const std::vector<double>& knots)
{
	mOriKnots = knots;
	//for (size_t i = 0; i < mOriKnots.size(); ++i)
	//{
	//	mOriKnots[i] *= mPeriod;
	//}
}
void SineWarp::SetCurrentKnotsOnTimeAxis(const std::vector<double>& knots)
{
	mCurKnots = knots;
	//for (size_t i = 0; i < mOriKnots.size(); ++i)
	//{
	//	mCurKnots[i] *= mPeriod;
	//}
}

double SineWarp::getBeizierValue(double fraction, const std::vector<double>& knots)
{
	double value = pow(1 - fraction, 5) * 0 
		+ 5 * fraction * pow(1 - fraction, 4) * knots[0]
	+ 10 * pow(fraction, 2) * pow(1 - fraction, 3) * knots[1]
	+ 10 * pow(fraction, 3) * pow(1 - fraction, 2) * knots[2]
	+ 5 * pow(fraction, 4) * (1 - fraction) * knots[3]
	+ pow(fraction, 5) * 1.0;
	return value;
}

double SineWarp::bisectionSearch(double searchValue, double minValue, double maxValue)
{
	for (int i = 0; i < 20; ++i)
	{
		double midValue = (minValue + maxValue) / 2.0;
		double testValue = getBeizierValue(midValue, mOriKnots);
		if (searchValue == testValue)
			return midValue;
		else if (searchValue > testValue)
		{
			minValue = midValue;
		}
		else
		{
			maxValue = midValue;
		}
	}
	return (minValue + maxValue) / 2.0;
}

double SineWarp::evaluateValueBeizier(double time)
{
	time += mPhase * mPeriod / (2 * PI);
	double fractionOfPeriod = rectifyTimeToFractionOfPeriod(time);
	if (!mOriKnots.size())
		return EvaluateSine(fractionOfPeriod);
	double beizierFraction = bisectionSearch(fractionOfPeriod, 0.0, 1.0);
	double realFractionOfPeriod = getBeizierValue(beizierFraction, mCurKnots);
	return EvaluateSine(realFractionOfPeriod);
}

void SineWarp::SetParametersGreg(double param1, double param2, double param3)
{
	mCurKnots.clear();
	mCurKnots.push_back(param1);
	mCurKnots.push_back(param2);
	mCurKnots.push_back(param3);

	mOriKnots.clear();
	mOriKnots.push_back(param1);
	mOriKnots.push_back(param2);
	mOriKnots.push_back(param3);
}

double SineWarp::bias(double b, double t)
{
	double ex = log(b) / log(0.5);
	return (pow(t, ex));
}

double SineWarp::gain(double g, double t)
{
	if (t < 0.5)
		return (0.5 * bias(1 - g, 2 * t));
	else
		return (1 - 0.5 * bias(1 - g, 2 - 2 * t));
}

double SineWarp::evaluateValueGreg(double time)
{
	time += mPhase * mPeriod / (2 * PI);
	//time -= 0.25 * mPeriod;
	double t = rectifyTimeToFractionOfPeriod(time);
	if (!mOriKnots.size())
		return EvaluateSine(t);


	double param1 = mCurKnots[0];
	double param2 = mCurKnots[1];
	double param3 = mCurKnots[2];
	
	if (param3 < 0.5)
	{
		float p3 = 1 - param3;
		t += 0.5;
		if (t > 1)
			t -= 1;
		t = gain(p3, t);
		t -= 0.5;
		if (t < 0) 
			t += 1;
	}
	else
	{
		t = gain(param3, t);
	}
	if (t < 0.5)
		t = 0.5 * gain(param1, 2 * t);
	else
		t = 0.5 + 0.5 * gain(param2, 2 * (t - 0.5));

	return EvaluateCosine(t);
}

double SineWarp::evaluateValueEdward(double time)
{
	if(mMod)
	{
		if(SecondHalfCycle(2.0 * time + 2.0 * (mPhase + 1.5708) * mTotalPeriod / (2 * PI)))
		{
			time = TimeWarp(time);
			time += mPhase * mTotalPeriod / (2 * PI);
			double t = rectifyTimeToFractionOfPeriod(time);
			return EvaluateSine(t);
		}

		time *= 2.0;
		time += mTotalPeriod * 0.75;
		time += 2.0 * mPhase * mTotalPeriod / (2 * PI);
		bool bSec = SecondHalfCycle(time - mTotalPeriod * 0.75);
		double t = rectifyTimeToFractionOfPeriod(time);


		if(bSec)
			return -EvaluateSineMod(t) - 0.5 * mAmp + 2.0 * mConst;
		return EvaluateSineMod(t) + 0.5 * mAmp;
	}
	else
	{
		time += mStraightLength / 2;
		time += 6.28 * mTotalPeriod / (2 * PI);
		time += mPhase * mTotalPeriod / (2 * PI);
		time = TimeWarp3(TimeWarp2(time - 4.71 * mTotalPeriod / (2 * PI)));
		time += 4.71 * mTotalPeriod / (2 * PI);
		double t = rectifyTimeToFractionOfPeriod(time);

		return EvaluateSine(t);
	}
}

double SineWarp::TimeWarp(double time)
{
	const double dPhaseShift = mPhase * mTotalPeriod / (2 * PI);
	time += dPhaseShift;

	double halfSinePeriod = 0.5 * mPeriod;
	double invHalfPeriod = 1.0 / (0.5 * (mTotalPeriod));
	time -= halfSinePeriod * 0.5;

	double overPeriod = time * invHalfPeriod;
	int flr = floor(overPeriod);
	double frac = overPeriod - flr;

	if(frac < mStraightLength * invHalfPeriod)
		return flr * halfSinePeriod + halfSinePeriod * 0.5 - dPhaseShift;
	else
		return flr * halfSinePeriod + (frac - mStraightLength * invHalfPeriod) / invHalfPeriod + halfSinePeriod * 0.5 - dPhaseShift;
}

double Lerp(double min, double max, double lin)
{
	return min * (1.0 - lin) + max * lin;
}

double SineWarp::TimeWarp2(double time)
{
	int nCycles = static_cast<int>(time / mTotalPeriod);
	double frac = (time - nCycles * mTotalPeriod) / mTotalPeriod;

	if(frac < mHalfRatio)
		frac = Lerp(0.0, 0.5, frac / mHalfRatio);
	else
		frac = Lerp(0.5, 1.0, (frac - mHalfRatio) / (1.0 - mHalfRatio));

	return (nCycles + frac) * mTotalPeriod;
}

double SineWarp::TimeWarp3(double time)
{
	int nCycles = static_cast<int>(time / (mTotalPeriod * 0.5));
	double frac = (time - nCycles * mTotalPeriod * 0.5) / (mTotalPeriod * 0.5);

	double ratio = 2.0 * mStraightLength / mTotalPeriod;


	if(frac < ratio)
		frac = 0.0;
	else
		frac = Lerp(0.0, 1.0, (frac - ratio) / (1.0 - ratio));

	return (nCycles + frac) * mTotalPeriod * 0.5;
}


double SineWarp::rectifyTimeToFractionOfPeriod(double time)
{
	while (time < 0)
		time += mTotalPeriod;
	if (time >= mTotalPeriod)
	{
		int nCycles = static_cast<int>(time / mTotalPeriod);
		time -= nCycles * mTotalPeriod;
	}
	return time / mTotalPeriod;
}
double SineWarp::evaluateValueTimeKnots(double time)
{
	time += mPhase * mPeriod / (2 * PI);
	double fractionOfPeriod = rectifyTimeToFractionOfPeriod(time);

	if (!mOriKnots.size())
		return EvaluateSine(fractionOfPeriod);
	assert(mOriKnots.size() == mCurKnots.size());
	int numKnots = static_cast<int>(mOriKnots.size());

	if (mCurKnots[0] > fractionOfPeriod)
	{
		double realFractionOfPeriod = fractionOfPeriod * mOriKnots[0] / mCurKnots[0];
		return EvaluateSine(realFractionOfPeriod);

	}
	else
	{
		for (int i = 0; i < numKnots - 1; ++i)
		{
			if (mCurKnots[i] <= fractionOfPeriod && mCurKnots[i + 1] > fractionOfPeriod)
			{
				double realFractionOfPeriod = mOriKnots[i] + (fractionOfPeriod - mCurKnots[i]) * (mOriKnots[i + 1] - mOriKnots[i]) / (mCurKnots[i + 1] - mCurKnots[i]);
				return EvaluateSine(realFractionOfPeriod);
			}
		}

		if (fractionOfPeriod >= mCurKnots[numKnots - 1])
		{
			double realFractionOfPeriod = mOriKnots[numKnots - 1] + (fractionOfPeriod - mCurKnots[numKnots - 1]) * (1.0 - mOriKnots[numKnots - 1]) / (1.0 - mCurKnots[numKnots - 1]);
			return EvaluateSine(realFractionOfPeriod);
		}
		else
		{
			(*DecoLogger::GetSingleton()) << "Time cannot be found in a warped sin wave!!! Time: " << time << ".\n";
			*((int*)0) = 0;
		}

	}
	return 0.0;

}

double SineWarp::EvaluateValue(double time)
{
	return evaluateValueEdward(time);
}
double SineWarp::EvaluateDerivative(double time)
{
	double deltaT = 0.0001;
	double value1 = EvaluateValue(time - deltaT);
	double value2 = EvaluateValue(time + deltaT);
	return (value2 - value1) / (2 * deltaT);
}

bool SineWarp::SecondHalfCycle(double time)
{
	int nCycles = static_cast<int>(time / double(mPeriod));

	return nCycles % 2;
}

double SineWarp::EvaluateSine(double fractionOfPeriod)
{
	double time = mPeriod * fractionOfPeriod;
	double omega = 2 * PI / mPeriod;

	return mAmp * sin(omega * time) + mConst;
}

double SineWarp::EvaluateSineMod(double fractionOfPeriod)
{
	double time = mPeriod * fractionOfPeriod;
	double omega = 2 * PI / mPeriod;

	return mAmp * 0.5 * sin(omega * time) + mConst;
}

double SineWarp::EvaluateCosine(double fractionOfPeriod)
{
	double time = mPeriod * fractionOfPeriod;
	double omega = 2 * PI / mPeriod;
	return mAmp * cos(omega * time) + mConst;
}
